//
// Created by xujingyi on 2021/4/28.
//
#include "SENTRYTHREAD.h"
#include "solve/Predictor.h"
#include "solve/SmoothKF.h"
#include "matplotlibcpp.h"
#include "plot.h"
#include <opencv2/highgui.hpp>
#include <iostream>
#include <fstream>
using namespace std;


uint8_t gFromCamera;
int gSrcNum;
int last_fps=100;
Serial serial(115200);
rm::Setter rm_setter;
WrapperHead *video = nullptr;    // 云台摄像头视频源
rm::ArmorFinder armor_finder(rm::ArmorColor(rm_setter.fixed_enemy_color),serial);
rm::Armor tmparmor=rm::Armor();
rm::Predictor predictor;
rm::GyroPredictor gyro_pretr;
rm::Smoother smoother;
#ifdef SAVE_VIDEO_PATH
static cv::VideoWriter video_writer = initVideoWriter(SAVE_VIDEO_PATH);
#endif
String Num_Path1,Num_Path2,Wrong_Path,Show_Path,Miss_Path;

int camera_buffer=-1,img_buffer=-1,deal_buffer=-1,show_buffer=-1,serial_catch_num=-1;
cv::Mat warp_img;//不能改！
cv::Mat origin,pre_pic,binary_pic,show_pic;
static cv::Mat S_camera_buffer,S_img_buffer,S_deal_buffer,S_show_buffer;//S_camera_buffer实际上是相机init时候开辟的缓冲区
std::mutex camera_mu,img_mu,deal_mu,show_mu,rxpackage_mu;
std::condition_variable condp1,condc1,condp2,condp2_c1,condc1_p2,condc2,condp3_c2,condc2_p3,condc2_3;
uint8_t rxpackage_is_valid=1,rxpackage_is_waited=0,copy_is_ok=0,transform_is_ok=0,deal_is_ok=0,camera_is_valid=0,camera_is_waited=0,shoot_num=0;
cv::Mat background = cv::Mat::zeros(PICTURE_ROW_NUM, 2 * PICTURE_COL_NUM, CV_64FC3);
Mat testboard = Mat::zeros(PICTURE_ROW_NUM, PICTURE_COL_NUM, CV_32FC3);
bool is_camera_been_flash=false;

static int num=0;

void showText(const cv::Mat &src){
    char shootflag[100],cstate[50];
    sprintf(shootflag, "shootflag:%i", armor_finder.getShootflag());
    sprintf(cstate, "state(S:0 T:1 G:2):%i",armor_finder.state);
    putText(src,shootflag,Point(10, 60), cv::FONT_HERSHEY_TRIPLEX, 0.8, COLOR_ORANGE/5);
    putText(src,cstate,Point(180, 60), cv::FONT_HERSHEY_TRIPLEX, 0.8, COLOR_ORANGE/5);
}

void saveVideos(const cv::Mat &src) {
    num>2000?(num=0):(num++);
    if (!src.empty()&&(num%rm_setter.save_internal==0)) {
#ifdef SAVE_VIDEO_PATH
        video_writer<<src;
#endif
    }
}

void showSrcNum(){
    if(gSrcNum>2147483640) gSrcNum=0;
    else gSrcNum++;
    if(rm_setter.show_src_cnt) LOGS("******************************PICTURE %d******************************",gSrcNum);
}

bool imgDeal1(){
//    if(gFromCamera==1) cameraExtract(S_deal_buffer);
//    else    videoExtract(S_deal_buffer);
    if(cameraExtract(S_deal_buffer)==false) return false;
    armor_finder.findTarget(S_deal_buffer);
    return true;
}

void imgDeal2(){
    armor_finder.solvePosNSend(S_deal_buffer);
    showText(show_pic);
}

void shoot2CameraBuffer(rm::Setter &tempSetter,WrapperHead *&tempVideo){
//    VideoWriter writer("test.avi", VideoWriter::fourcc('M', 'J', 'P', 'G'), 30.0, Size(640, 480), true);
    while(1){
        std::unique_lock<std::mutex> camera_locker(camera_mu);
        while((camera_is_waited==1)&&(is_camera_been_flash!=false)) {
            condc1.wait(camera_locker);
        }
#ifdef TEST_THREAD
        camera_buffer++;
        cout<<"1:get a pic:"<<camera_buffer<<endl;
#endif
        camera_is_valid=0;

        showSrcNum();

        if((gFromCamera==1)&&(!checkReconnect(tempVideo->read(S_img_buffer),video))) continue;

        else tempVideo->read(S_img_buffer);


#ifdef TEST_THREAD
        if(camera_buffer++>10000) camera_buffer=0;
        cout<<"1:get a pic[END]:"<<camera_buffer<<endl;
#endif

        camera_is_valid=1;
        is_camera_been_flash=true;
        condp1.notify_one();
        camera_locker.unlock();
    }
}

void transport2CPUBuffer(rm::Setter &tempSetter,WrapperHead *&tempVideo){
    while (1) {
        std::unique_lock<std::mutex> img_locker(img_mu);
        while (transform_is_ok == 1) {
            condp2_c1.wait(img_locker);
        }

        camera_is_waited=1;
        std::unique_lock<std::mutex> camera_locker(camera_mu);
        while(camera_is_valid==0) {
            condp1.wait(camera_locker);
        }
        if ((is_camera_been_flash==false)) continue;

#ifdef TEST_THREAD
        img_buffer=camera_buffer;
        cout << "2:camera-cpu:" << img_buffer << endl;
#endif

        camera_is_waited=0;
        is_camera_been_flash= false;
        condc1.notify_one();
        camera_locker.unlock();

        transform_is_ok=1;
        condc1_p2.notify_one();
        img_locker.unlock();
    }
}

void copy2DealBuffer(){
    while(1){
        std::unique_lock<std::mutex> img_locker(img_mu);
        while(transform_is_ok==0){
            condc1_p2.wait(img_locker);
        }

        std::unique_lock<std::mutex> deal_locker(deal_mu);
        while(copy_is_ok==1){
            condc2.wait(deal_locker);
        }
        S_deal_buffer=S_img_buffer.clone();

#ifdef TEST_THREAD
        deal_buffer=img_buffer;
        cout<<"3:picture copy:"<<deal_buffer<<endl;
#endif

        copy_is_ok=1;
        condp2.notify_one();
        deal_locker.unlock();

        transform_is_ok=0;
        condp2_c1.notify_one();
        img_locker.unlock();
    }
}

void imageDeal(rm::Setter &tempSetter,WrapperHead *&tempVideo ,Serial &tempSerial){
    systime t1,t2;
    while(1){
        std::unique_lock<std::mutex> deal_locker(deal_mu);
        while(copy_is_ok==0)
        {
            condp2.wait(deal_locker);
        }
        getsystime(t1);
#ifdef TEST_THREAD
        cout<<"4:dealing:"<<deal_buffer<<endl;
#endif
        if(!imgDeal1()) continue;
#ifdef TEST_THREAD
        cout<<"4:dealing[END1]:"<<deal_buffer<<endl;
#endif

//        rxpackage_is_waited=1;
//        std::unique_lock<std::mutex> rxpackage_locker(rxpackage_mu);
//        while(rxpackage_is_valid==0) {
//            condp3_c2.wait(rxpackage_locker);
//        }
        LOGT("imageDeal:%.1fms",getTimeIntervalms(getsystime(t2),t1));
        //cout<<"5:serial_pic:"<<deal_buffer<<endl;
        imgDeal2();
        if(rm_setter.show_track_box) cv::rectangle(show_pic,armor_finder.getTrackBox(),COLOR_VIOLET/5,1.5);
        std::unique_lock<std::mutex> show_locker(show_mu);
        showSerial(show_pic,serial.txpackage,serial.rxpackage);
        S_show_buffer=show_pic.clone();
        show_buffer=deal_buffer;
        tmparmor=armor_finder.target_armor;
        #ifdef TEST_THREAD
        cout<<"4:dealing[END]:"<<deal_buffer<<endl;
#endif

        deal_is_ok=1;
        condc2_3.notify_one();
        show_locker.unlock();

//        rxpackage_is_waited=0;
//        condc2_p3.notify_one();
//        rxpackage_locker.unlock();

        copy_is_ok=0;
        condc2.notify_one();
        deal_locker.unlock();

    }
}

void imageShow(rm::Setter &tempSetter){
    cv::namedWindow("show_pic", cv::WINDOW_NORMAL);
    Plot plt(500);
    ofstream fin;
//    if(access(INFO_OUT_PATH,F_OK)==-1) mkdir(INFO_OUT_PATH,S_IRWXO|S_IRWXG|S_IRWXU);
//    fin.open(INFO_OUT_PATH"/draw.txt",std::ios::trunc);
    systime t1,t2;
    while(1){
        unsigned int plot_cnt;
        if(plot_cnt>5000) plot_cnt=0;
        std::unique_lock<std::mutex> show_locker(show_mu);
        while(deal_is_ok==0) {
            condc2_3.wait(show_locker);
        }
#ifdef TEST_THREAD
        cout<<"5:show:"<<show_buffer<<endl;
#endif
        getsystime(t1);
//        cv::Point3f pos= tmparmor.box_pos.getPosMessage();
//        fin << (double)getsystime() <<","<<pos.x <<","<< pos.y << "," << pos.z << "," <<std::endl;//<< pos.pitch_predict << "," << pos.yaw_predict << "," << std::endl;
        S_show_buffer=S_show_buffer*5;
        if (rm_setter.show_preprocess&&!pre_pic.empty()) imshow("pre_pic", pre_pic);
        if (rm_setter.show_origin&&!origin.empty()) showOrigin(origin);

        showFps(S_show_buffer);

        if(rm_setter.show_warp_num&&!warp_img.empty()) imshow("warp_num",warp_img);
        if (rm_setter.show_armor) {                 // 根据条件显示当前目标装甲板
//            showArmorBox("target_box", S_show_buffer, armor_finder.target_box);
        }
        if(rm_setter.show_midline) {
//            line(S_show_buffer,cv::Point(0,PICTURE_ROW_NUM/2),cv::Point(PICTURE_COL_NUM,PICTURE_ROW_NUM/2),COLOR_YELLOW,2);
//            line(S_show_buffer,cv::Point(PICTURE_COL_NUM/2,0),cv::Point(PICTURE_COL_NUM/2,PICTURE_ROW_NUM),COLOR_BLUE,2);
//            circle(S_show_buffer,cv::Point(PICTURE_COL_NUM/2,PICTURE_ROW_NUM/2),5,COLOR_RED,-1);
            line(S_show_buffer,cv::Point(0,IMAGE_CENTER_Y),cv::Point(PICTURE_COL_NUM,IMAGE_CENTER_Y),COLOR_YELLOW,2);
            line(S_show_buffer,cv::Point(IMAGE_CENTER_X,0),cv::Point(IMAGE_CENTER_X,PICTURE_ROW_NUM),COLOR_GREEN,2);
            circle(S_show_buffer,cv::Point(IMAGE_CENTER_X,IMAGE_CENTER_Y),5,COLOR_RED,-1);
        }
        if(rm_setter.save_show_picture){
            static int show_pic_cnt=0;
            std::string num;
            num=Show_Path+"/picture_"+ to_string(show_pic_cnt) +".jpg";
            if (show_pic_cnt%rm_setter.save_internal==0) cv::imwrite(num,S_show_buffer);
            show_pic_cnt++;
        }
        if(!S_show_buffer.empty()) cv::imshow("show_pic",S_show_buffer);
        if (rm_setter.show_testboard)cv::imshow("test",testboard);
        if(rm_setter.show_plot){
            cv::Point3f pos= tmparmor.box_pos.getPosMessage();
            plt.record(pos.x, "pitch");
            plt.record(pos.y, "yaw");
            plt.record(pos.z, "dis");
            if(plot_cnt++%10==0) plt.show();
        }

        if (!rm_setter.run_by_frame) cv::waitKey(1);

#ifdef SAVE_VIDEO_PATH
        saveVideos(show_pic*5);
#endif
#ifdef TEST_THREAD
        cout<<"5:show[END]:"<<show_buffer<<endl;
#endif
        LOGT("imageShow:%.1fms",getTimeIntervalms(getsystime(t2),t1));
        deal_is_ok=0;
        condc2_3.notify_one();
        show_locker.unlock();
//        while (rxpackage_is_waited == 0) continue;
        //cout<<"SERIAL_CATCH:deal_num:"<<deal_num<<"  serial_num:"<<serial_catch_num++<<endl;
    }
//    fin.close();
}

void readSerial(Serial &tempSerial){
    systime t1,t2;
    while(1){
//        std::unique_lock<std::mutex> rxpackage_locker(rxpackage_mu);
//
//        while (rxpackage_is_waited == 1) {
//            condc2_p3.wait(rxpackage_locker);
//        }
        rxpackage_is_valid=0;
#ifndef DEBUG_MODE
        rm_setter.wait_uart=1;
#endif
#ifdef TEST_THREAD
        if(serial_catch_num++>1000000) serial=0;
        cout<<"serial_num:"<<serial_catch_num<<endl;
#endif
        //if(!armor_finder.readMessage()) cerr<<"[ERROR]Can't read serial!!"<<endl;
        getsystime(t1);
        if(rm_setter.wait_uart&&!readTarget(tempSerial)) cerr << "[ERROR]Can't read serial!!" << endl;
        LOGT("serial:%.1fms",getTimeIntervalms(getsystime(t2),t1));
        if(!rm_setter.wait_uart) armor_finder.setEnemyColor(rm::ArmorColor(rm_setter.fixed_enemy_color));
        else armor_finder.setEnemyColor(rm::ArmorColor(serial.rxpackage.enemy_color));
#ifdef TEST_THREAD
//        cout<<"serial_num[END]:"<<serial_catch_num<<endl;
#endif

        rxpackage_is_valid=1;
//        condp3_c2.notify_one();
//        rxpackage_locker.unlock();

//        while (rxpackage_is_waited == 1) ;
    }
}//p3

void sentryFight(rm::Setter &tempSetter,WrapperHead *&tempVideo ,Serial &tempSerial,int enemy_color) {
    //std::ref 用于包装按引用传递的值。（类似&）它不是引用，只是把变量包装成引用的形式而以。
    //std::cref 用于包装按const引用传递的值
    //函数模板 ref 与 cref 是生成 std::reference_wrapper 类型对象的帮助函数，它们用模板实参推导确定结果的模板实参。所以std::ref()返回的实际上是一个reference_wrapper而不是T&，
    // 可以从一个指向不能拷贝的类型的对象的引用生成一个可拷贝的对象。 std::reference_wrapper 的实例是对象（它们可被复制或存储于容器），但它们能隐式转换成 T& ，
    // 故能以之为以引用接收底层类型的函数的参数。

    //std::thread videoDataThread(infantryVideoDataSave);
    std::thread shoot2CameraBufferThread(shoot2CameraBuffer,std::ref(tempSetter),std::ref(tempVideo));
    std::thread transport2CPUBufferThread(transport2CPUBuffer,std::ref(tempSetter),std::ref(tempVideo));
    std::thread copy2DealBufferThread(copy2DealBuffer);
    std::thread imageDealThread(imageDeal,std::ref(tempSetter),std::ref(tempVideo),std::ref(tempSerial));
    std::thread imageShowThread(imageShow,std::ref(tempSetter));
    std::thread readSerialThread(readSerial,std::ref(tempSerial));
    //当使用join()函数时，主调线程阻塞，等待被调线程终止，然后主调线程回收被调线程资源，并继续运行；
    //所以 readThread、imageDealThread、videoDataThread、writeThread函数会同步运行，infantryFight会等到这四个函数都结束了才会继续执行

    readSerialThread.join();
    shoot2CameraBufferThread.join();
    transport2CPUBufferThread.join();
    copy2DealBufferThread.join();
    imageDealThread.join();
    imageShowThread.join();

}
